/*********************
** 操作共享内存
**********************/
#include <sys/shm.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "zuc/motion/semintf.h"
#include "common_def_sensor.h"
#include "torq_mem_operate.h"

//create a semaphore
int sem_get(key_t key)
{
    int sem_id = semget(key, 1, 0666 | IPC_CREAT);
    union semun sem_union;
    sem_union.val = 1;
    if (semctl(sem_id, 0, SETVAL, sem_union) == -1)
    {
        return -1;
    }
    return sem_id;
}

//obtain a semaphore
int sem_p(int sem_id)
{
    struct sembuf sem_buf;
    sem_buf.sem_op = -1;
    sem_buf.sem_flg = SEM_UNDO;
    sem_buf.sem_num = 0;
    if (semop(sem_id, &sem_buf, 1) == -1)
    {
        return -1;
    }
    return 0;
}

//release an semaphore
int sem_v(int sem_id)
{
    struct sembuf sem_buf;
    sem_buf.sem_flg = SEM_UNDO;
    sem_buf.sem_num = 0;
    sem_buf.sem_op = 1;
    if (semop(sem_id, &sem_buf, 1) == -1)
    {
        return -1;
    }
    return 0;
}

void setSensorInitError()
{
    printf("setSensorInitError\n");
    if (!sensorShmStruct)
    {
        return;
    }
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        sensorShmStruct->sensorStatus.status = STATUS_ERROR;
        sensorShmStruct->sensorStatus.errCode = ERRRO_INIT;
        sem_v(sem_id);
    }
}

void setSensorReadNoData()
{
    printf("setSensorReadNoData\n");
    if (!sensorShmStruct)
    {
        return;
    }
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        sensorShmStruct->sensorStatus.status = STATUS_ERROR;
        sensorShmStruct->sensorStatus.errCode = ERRRO_NO_DATA;
        sem_v(sem_id);
    }
}

void setSensorReadWrongData()
{
    printf("setSensorReadWrongData\n");
    if (!sensorShmStruct)
    {
        return;
    }
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        sensorShmStruct->sensorStatus.status = STATUS_ERROR;
        sensorShmStruct->sensorStatus.errCode = ERRRO_WRONG_DATA;
        sem_v(sem_id);
    }
}

void setSensorConnectSucc()
{
    printf("setSensorConnectSucc\n");
    if (!sensorShmStruct)
    {
        return;
    }
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        sensorShmStruct->sensorStatus.status = STATUS_OPENING;
        sensorShmStruct->sensorStatus.errCode = ERRRO_NONE;
        sem_v(sem_id);
    }
}

void setSensorConnectClose()
{
    printf("setSensorConnectClose\n");
    if (!sensorShmStruct)
    {
        return;
    }
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        sensorShmStruct->sensorStatus.status = STATUS_CLOSING;
        sensorShmStruct->sensorStatus.errCode = ERRRO_NONE;
        sensorShmStruct->sensorConfig.mode = MODE_CLOSE;
        sem_v(sem_id);
    }
}

void setSensorData(const double* sensor_data)
{
    if (!sensorShmStruct)
    {
        return;
    }
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        if (sensorShmStruct->sensorConfig.torqsensorBrand != 5)
        {
            for (int i = 0; i < 6; i++) { sensorShmStruct->sensorStatus.torque[i] = sensor_data[i]; }
        }
        else
        {
            // 5型1维传感器使用的时Fx通道的数据
            for (int i = 0; i < 6; i++) { sensorShmStruct->sensorStatus.torque[i] = 0.0; }
            sensorShmStruct->sensorStatus.torque[2] = sensor_data[0];
        }
        sem_v(sem_id);
    }
}

int sensorBrand()
{
    if (!sensorShmStruct)
    {
        return -1;
    }
    int brand = -1;
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        brand = sensorShmStruct->sensorConfig.torqsensorBrand;
        sem_v(sem_id);
    }
    return brand;
}

// 0 关闭 1 打开
double sensorMode()
{
    if (!sensorShmStruct)
    {
        return 0;
    }
    int mode = 0;
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        mode = sensorShmStruct->sensorConfig.mode;
        sem_v(sem_id);
    }
    return mode;
}

// 0 关闭 1 打开 2 错误
int sensorStatus()
{
    if (!sensorShmStruct)
    {
        return 0;
    }
    int status = 0;
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        status = sensorShmStruct->sensorStatus.status;
        sem_v(sem_id);
    }
    return status;
}

// 0 不滤波
double sensorFilter()
{
    if (!sensorShmStruct)
    {
        return 0;
    }
    double filter = 0;
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        filter = sensorShmStruct->torqueSensorFilter;
        sem_v(sem_id);
    }
    return filter;
}
